#include <stdio.h>
#include "rtps/common/types.h"
#include "rtps/config.h"
#include "rtps/messages/MessageTypes.h"
#include "rtps/utils/sysFunctions.h"
#include "rtps/storages/PBufWrapper.h"

#include "rtps/messages/MessageFactory.h"

const char PROTOCOL_TYPE[4] = {'R', 'T', 'P', 'S'};
//const std::array<uint8_t, 4> PROTOCOL_TYPE{'R', 'T', 'P', 'S'};
const uint8_t numBytesUntilEndOfLength = 4; // The first bytes incl. submessagelength don't count

void rtps_MessageFactory_addHeader(struct rtps_PBufWrapper *buffer, const struct GuidPrefix_t *guidPrefix)
{
  struct Header header;
  //header.protocolName = PROTOCOL_TYPE;
  memcpy(header.protocolName, PROTOCOL_TYPE, 4);
  header.protocolVersion = PROTOCOLVERSION;
  header.vendorId = VENDOR_ID;
  header.guidPrefix = *guidPrefix;

  serializeMessageHeader(buffer, &header);
}
/*
bool AddSubMessageInfoDST(Buffer *buffer, struct GuidPrefix_t *dst)
{
  struct SubmessageInfoDST msg;
  msg.header.submessageId = INFO_DST;

#if IS_LITTLE_ENDIAN
  msg.header.flags = FLAG_LITTLE_ENDIAN;
#else
  msg.header.flags = FLAG_BIG_ENDIAN;
#endif

  msg.header.octetsToNextHeader = sizeof(GuidPrefix_t);
  msg.guidPrefix = dst;

  return serializeMessageSubmessageInfoDST(buffer, msg);
}
*/
void rtps_MessageFactory_addSubMessageTimeStamp(struct rtps_PBufWrapper *buffer, bool setInvalid)
{
  struct SubmessageHeader header;
  header.submessageId = INFO_TS;

#if IS_LITTLE_ENDIAN
  header.flags = FLAG_LITTLE_ENDIAN;
#else
  header.flags = FLAG_BIG_ENDIAN;
#endif
  if (setInvalid) {
    header.flags |= FLAG_INVALIDATE;
    header.octetsToNextHeader = 0;
  } else {
    header.octetsToNextHeader = sizeof(Time_t);
  }

  serializeMessageSubmessageHeader(buffer, header);

  if (!setInvalid) {
    rtps_PBufWrapper_reserve(buffer, header.octetsToNextHeader);
    Time_t now;
    getCurrentTimeStamp(&now);
    rtps_PBufWrapper_append(buffer, (const uint8_t *)(&now.seconds),
                  sizeof(now.seconds));
    rtps_PBufWrapper_append(buffer, (const uint8_t *)(&now.fraction),
                  sizeof(now.fraction));
  }
}

void rtps_MessageFactory_addSubMessageData(struct rtps_PBufWrapper *buffer, struct rtps_PBufWrapper *filledPayload,
                       bool containsInlineQos, const struct SequenceNumber_t *SN,
                       const struct EntityId_t *writerID, const struct EntityId_t *readerID)
{
  struct SubmessageData msg;
  msg.header.submessageId = DATA;
#if IS_LITTLE_ENDIAN
  msg.header.flags = FLAG_LITTLE_ENDIAN;
#else
  msg.header.flags = FLAG_BIG_ENDIAN;
#endif

  msg.header.octetsToNextHeader = SubmessageDataGetRawSize() +
                                  rtps_PBufWrapper_spaceUsed(filledPayload) -
                                  numBytesUntilEndOfLength;

  if (containsInlineQos) {
    msg.header.flags |= FLAG_INLINE_QOS;
  }
  if (rtps_PBufWrapper_isValid(filledPayload)) {
    msg.header.flags |= FLAG_DATA_PAYLOAD;
  }
  msg.writerSN = *SN;
  msg.extraFlags = 0;
  msg.readerId = *readerID;
  msg.writerId = *writerID;

  const uint16_t octetsToInlineQoS = 4 + 4 + 8; // EntityIds + SequenceNumber
  msg.octetsToInlineQos = octetsToInlineQoS;

  serializeMessageSubmessageData(buffer, &msg);

  if (rtps_PBufWrapper_isValid(filledPayload)) {
    struct rtps_PBufWrapper *shallowCopy;
    shallowCopy = rtps_PBufWrapper_Construct(RTPS_PBW_BY_LENG, rtps_PBufWrapper_spaceUsed(filledPayload));
    rtps_PBufWrapper_append(shallowCopy, filledPayload->firstElement->payload, rtps_PBufWrapper_spaceUsed(filledPayload));
    rtps_PBufWrapper_append_cat(buffer, shallowCopy);
    rtps_PBufWrapper_Destruct(shallowCopy);
  }
}

void rtps_MessageFactory_addHeartbeat(struct rtps_PBufWrapper *buffer, 
struct EntityId_t *writerId, struct EntityId_t *readerId,
                  struct SequenceNumber_t *firstSN, struct SequenceNumber_t *lastSN,
                  Count_t count)
{
  struct SubmessageHeartbeat subMsg;
  subMsg.header.submessageId = HEARTBEAT;
  subMsg.header.octetsToNextHeader =
      SubmessageHeartbeatGetRawSize() - numBytesUntilEndOfLength;
#if IS_LITTLE_ENDIAN
  subMsg.header.flags = FLAG_LITTLE_ENDIAN;
#else
  subMsg.header.flags = FLAG_BIG_ENDIAN;
#endif
  // Force response by not setting final flag.

  subMsg.writerId = *writerId;
  subMsg.readerId = *readerId;
  subMsg.firstSN = *firstSN;
  subMsg.lastSN = *lastSN;
  subMsg.count = count;

  serializeMessageSubmessageHeartbeat(buffer, &subMsg);
}

void rtps_MessageFactory_addAckNack(struct rtps_PBufWrapper *buffer, struct EntityId_t *writerId, struct EntityId_t *readerId, struct SequenceNumberSet *readerSNState, Count_t count, bool final_flag)
{
  struct SubmessageAckNack subMsg;
  subMsg.header.submessageId = ACKNACK;
#if IS_LITTLE_ENDIAN
  subMsg.header.flags = FLAG_LITTLE_ENDIAN;
#else
  subMsg.header.flags = FLAG_BIG_ENDIAN;
#endif
  if (final_flag) {
    subMsg.header.flags |= FLAG_FINAL; // For now, we don't want any response
  } else {
    subMsg.header.flags &= ~FLAG_FINAL; // For now, we don't want any response
  }
  subMsg.header.octetsToNextHeader =
      SubmessageAckNackGetRawSize(*readerSNState) - numBytesUntilEndOfLength;

  //subMsg.writerId = writerId;
  //subMsg.readerId = readerId;
  //subMsg.readerSNState = readerSNState;
  memcpy(&(subMsg.writerId), writerId, sizeof(struct EntityId_t));
  memcpy(&(subMsg.readerId), readerId, sizeof(struct EntityId_t));
  memcpy(&(subMsg.readerSNState), readerSNState, sizeof(struct SequenceNumberSet));
  subMsg.count = count;

  serializeMessageSubmessageAckNack(buffer, subMsg);
}
